
/** IMPORTANT: CHECK COMPATIBILITY OF THIS CODE WITH YOUR CONTROLLERS, MOTORS, AND SENSORS BEFORE EXECUTION
// note: this was written for IBT 2Hcontroller 
// for "Motor controller DRV8835" code needs to be changed to work with "enable pin" and "phase pin"

/**********************************************************************************
                                  INITIALIZATION
 **********************************************************************************/

/*Motor controllers
***********************/
// note: speed is between 0 and 255
int motLforward = 10;  // forward pin of motor controller 1 
int motRforward = 5;  // forward pin of motor controller 2 
int motLbackward = 11;  // backward pin of motor controller 1 
int motRbackward = 6;  // backward pin of motor controller 2 
int slow_speed = 30; // for forward speed and turning

/*Line sensors
******************/
int LL_pin = 13; // line sensor input left wheel
int LR_pin = 12; // line sensor input right wheel
int LL = 0; // detection line sensor left wheel
int LR = 0; // detection line sensor right wheel
boolean lineDetected = 0;

/*Start button
*******************/
long countdowntime = 5000; // how long [msec] to wait till movement after pushing button

/*Finite state machine parameters
************************************/
int state = 0; // current state
int old_state = 1; // the previous state
int state_start = 0; // starting time of the current state
int state_duration = 0; // how long we have been in the current state
boolean new_state = true; // indicate whether we have transitioned to a new state 

int turn_maneuver_time_limit = 500; // beslissen wanneer we overgaan van state 11 naar state 12
boolean turn_manoever_left = true;

/**********************************************************************************
                                           SETUP
 **********************************************************************************/
 
 void setup() {   
  Serial.begin(9600);

  /*Motor controllers
  *********************************/
  pinMode(motLforward, OUTPUT);
  pinMode(motRforward, OUTPUT);
  pinMode(motLbackward, OUTPUT);
  pinMode(motRbackward, OUTPUT);

  /*Line sensors
  *********************************/
  pinMode(LL_pin, INPUT); 
  pinMode(LR_pin, INPUT);

/**********************************************************************************
                                     STARTUP PROCEDURE
**********************************************************************************/
  delay(countdowntime); // wait 5 seconds
  
  new_state = true;
  state_start = millis();
  state = 0;
} 

void loop()  { 
  
    /**********************************************************************************
                                     READ SENSORS
    **********************************************************************************/
 
  /*Line sensors
  ********************************/
  LL=digitalRead(LL_pin); 
  LR=digitalRead(LR_pin); 
 
  Serial.print(LL);
  Serial.print("\t");
  Serial.print(LR);
  Serial.print("\t");
    
     /**********************************************************************************
                                DETERMINE STATE (see state decision tree)
      **********************************************************************************/
  
  /*<...>*/
  
  old_state = state;
  state_duration = millis()-state_start;  // how long have we been in the current state
  Serial.print(state_duration);
  Serial.print("\t"); 
  Serial.print(millis());
  Serial.print("\t");  

  if (LL || LR) { // line detected
       lineDetected = 1;
       if (LL){
         state = 21;
         turn_manoever_left = 0;
       }
       else {
         state = 22;
         turn_manoever_left = 1;
       }    
  } 

  else {  // no line detected
      if ((state_duration < turn_maneuver_time_limit) && lineDetected) {
        if (turn_manoever_left) {
          state = 22;
        }
        else {
          state = 21;
        }
      }
      else {
        state = 11;
        lineDetected = 0;
      }
  }
  
  new_state = (state != old_state);
  if(new_state && state != 11) {
    state_start = millis();
    Serial.print("changed state");  
  }
  Serial.println(state);

  
      /**********************************************************************************
                                     EXECUTE STATE
      **********************************************************************************/

  switch(state) {
    
    /* Initial dummy state
    *********************/
    case 0: 
      // does nothing at the moment
    break;
 
    /**********************************************/
                    /* NORMAL
    **********************************************/
    // DRIVE FORWARD
    case 11:
        digitalWrite(motLforward, slow_speed);
        digitalWrite(motRforward, slow_speed);
        digitalWrite(motLbackward, 0);
        digitalWrite(motRbackward, 0);
    break;
         
    /***********************************/  
              /* TURN MANEUVERS
    ************************************/ 
    // Turn right
    case 21:
        digitalWrite(motLforward, slow_speed);
        digitalWrite(motRforward, 0);
        digitalWrite(motLbackward, 0);
        digitalWrite(motRbackward, slow_speed);       
        
      break;
    // Turn left
    case 22:
        digitalWrite(motLforward, 0);
        digitalWrite(motRforward, slow_speed);
        digitalWrite(motLbackward, slow_speed);
        digitalWrite(motRbackward, 0);
      break;
  }
  delay(1); //1ms delay in loop to allow state to be executed
}
